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CHAPTER  1 
INTRODDCTION 

American  industry,  as  a  whole,  and  the  automobile  industry  in 
particular,  has  been  investing  heavily  in  recent  years  to  modernize 
manufacturing  facilities,  to  lessen  manufacturing  costs,  and  to  improve 
quality.  Emphasis  has  been  placed  on  teachable  automation  in  an 
effort  to  ensure  that  new  equipment  will  be  flexible  enough  to 
accomodate  future  products  with  minimal  additional  investment.  Tlie 
primary  teachable  components  of  flexible  manufacturing  systems  are 
robots,  and  the  primary  component  of  a  robotic  system  that  allows  it  to 
be   teachable    is   computer  control. 

However,  computer  control  of  robots  is  not  well  refined  and  robots 
do  not  perform  upto  their  physical  capabilities.  Higher  performance 
robots  would  be  valuable  in  manufacturing.  For  example,  many 
applications  of  robots  in  factories  can  be  justified  economically  only 
if  implemented  with  a  faster  robot  than  state-of-the-art  control 
permits.  Other  applications  could  be  implemented  with  fewer  faster 
robots,  which  would  result  in  considerable  investment  savings.  Another 
performance  limitation,  besides  speed,  is  the  maximum  load  bearing 
capacity  of  commercially  available   robots. 
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These  performance  limitations  can  be  traced  to  the  manner  in  which 
existing  robots  are  controlled.  State-of-the-art  control  schemes  are 
based  solely  on  stability  requirements.  The  control  law  is  designed  to 
produce  stability  in  axis  position  and  then  the  fact  of  stability  itself 
is  used  to  induce  motion  by  iteratively  changing  the  position  reference. 
This  form  of  control  gives  rise  to  inaccuracy  at  high  speeds  and  to 
position  overshoots.  Consequently  robot  designers  have  restricted  the 
peak  speed  and  acceleration  of  their  products  so  that  accuracy  and 
overshoot  can  be  limited  to  acceptable  levels.  This  is  a  performance 
limitation  due  to  the  control  laws  and  not  due  to  the  capabilities  of 
the  machine.  Therefore,  to  control  the  robot  for  high  performance,  the 
true  physical  performance  limitations  must  first  be  established.  The 
limitations  are  then  based  on  constraints  and  not  merely  on  the 
control  [1]. 

A  real  life  example  will  illustrate  this  difference  in  performance 
levels.  The  standard  PUMA  arm  equipment  could  allow  base  motor  speeds 
of  up  to  144  rad/s,  and  currently,  the  limit  set  in  the  Puma  controller 
is  89.9  rad/s  [1].  A  60%  improvement  is  possible  if  the  full  potential 
of  the  motor  can  be  tapped. 

The  question  of  what  is  minimum  time  control  can  be  answered 
with  a  commonly  used  analogy.  If  a  person  vfere  travelling  in  a  car  and 
wished  to  get  to  the  next  intersection  in  the  shortest  possible  time, 
what  would  he  do?  He  would  push  the  accelerator  to  the  floor  for  a 
certain  amount  of  time  and  then  releasing  the  accelerator  apply  maximun 
braking  (switch  controls)  for  some  other  period  of  time  to  come  to  a 
stop.   If  the  control  switched  too  late,  the  car  would  slide  into  the 

-2- 


intersection.  If  it  switched  too  soon,  the  car  would  stop  short  of  the 
intersection.  Thus,  it  is  obvious  that  the  switching  time  is  critical 
in  obtaining  the  desired  final  position  and  velocity.  This  is  the  basic 
idea  in  the  minimum  time  optimal  control  of  a  robot  manipulator  or 
articulated  mechanism.  This  type  of  control  is  also  called  bang-bang 
control  [2],  because  the  control  is  always  on  the  control  boundary,  in 
one   direction  or   the   other. 

The  minimum  time,  optimal  control  problem  for  a  robotic 
manipulator  can  be  divided  into  two  main  classes,  minimum  time  control 
along  a  specified  path  and  minimum  time  control  with  no  path 
constraints.  Task  oriented  problems  and  obstacle  avoidance  planning 
fall  into  the  former  class.  The  second  class  can  be  subdivided  into  two 
categories,  that  of  problems  where  the  complete  nonlinear  minimum  time 
system  is  considered  and  the  true  solution  is  sought,  and  of  problems 
where  approximations  (usually  linearizations)  are  made  on  the  nonlinear 
system  and  the  near-minimum  time  control  is  investigated.  Extensive 
work  has  been  done  with  problems  belonging  to  the  first  class  [3]  -  [7]. 
Problems  dealing  with  the  near-minimum  time  control  have  also  been  quite 
extensively  investigated  in  recent  years.  In  1971  Kahn  and  Roth 
published  a  paper  on  the  near-minimum  time  control  of  open  loop 
articulated  kinematic  chains  [8].  They  developed  a  suboptimal  feedback 
control  by  linearizing  the  equations  of  motion  for  a  three  degree  of 
freedom  manipulator.  Approximations  were  made  for  the  effects  of 
gravity  loads  and  angular  velocities  in  the  nonlinear  dynamic  equations. 
The  suboptimal  control  was  obtained  by  decoupling  the  system  into  three 
double    integrators    and  deriving   the   equations   for   the    switching   curves 
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of  the  transformed  system.  The  response  time  of  the  snboptimal  control 
was  compared  to  that  of  the  optimal  control  which  was  obtained  by  an 
iterative  technique.  Wen  and  Desrochers  [9]  investigated  two  control 
strategies  for  suboptimal  control,  the  method  of  averaging  dynamics  (AD) 
and  the  method  of  linear  equivalence  (LE)  .  The  first  method  is  used 
when  a  time-fuel  suboptimal  solution  is  required.  The  latter  uses  exact 
linearization  where  the  dynamic  equations  are  written  for  a  reduced 
system  of  decoupled  double  integrators.  The  LE  method  is  found  to  be 
superior  to  the  AD  method  in  obtaining  a  smaller  final  time.  However 
both  methods  need  a  very  good  model  of  the  system  since  the  nonlinear 
part  of  the  system  has  to  be  evaluated  repetitively.  Sato,  Shimoj  ima 
and  Kitamura  [10]  obtained  switching  tines  of  the  control  variables  for 
a  two  degree  of  freedom  manipulator  by  approximating  the  velocity  of  a 
DC  servomotor.  They  found  that  when  the  driving  force  was  operating  at 
saturation  it  was  necessary  to  make  additional  approximations  on  the 
angular  velocities.  Kao,  Sinha  and  Mahalanabis  [11]  developed  an 
algorithm  for  the  near-minimum  time  control  of  a  three  link  robotic 
manipulator.  They  linearized  the  dynamic  equations  by  expanding  them  in 
a  Taylor  series  and  neglecting  the  higher  order  terms.  The  poles  of  the 
linearized  closed  loop  system  were  placed  in  the  z-plane  so  as  to  permit 
minimum  time  response  without  violating  the  actuator  torque  constraints. 
This  is  a  digital  algorithm  that  can  be  implemented  using 
microprocessors. 

However  very  little  work  has  been  done  in  the  area  of  the  complete 
problem,  the  problem  of  determining  the  minimum  time  optimal  control 
history  for  a  system  with  no  linearizations  or  approximations.   Kahn  and 
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Roth  [8]  obtained  the  minimum  time  optimal  control  by  an  iterative 
technique.  Guesses  were  made  on  the  unknown  constants  at  the  final  time 
X(t.),    and   the   dynamic   equations  were    integrated   backwards    in    time    to 

the    initial    time    to    give    the    states  x(t    )    and  the   constants  X(t    ).    If 

o  0 

x(t  )  is  not  sufficiently  close  to  the  specified  initial  state  x  then  a 
o  '^  o 

new  set  of  variables  3l(t.)  are  chosen  and  the  integration  is  repeated. 

The  iteration  is  continued  till  x(t  )  is  sufficiently  close  to  x  .  Then 

o  '  0 

the  constants  X(t    )  and  x   are  substituted  into  the  dynamic  equations 

and  the  optimal  control  is  obtained  by  integrating  the  equations  forward 
to  the  final  time. 

The  purpose  of  this  work  is  to  develop  an  alternate  method  of 
solving  the  complete  minimum  time  problem  ,  to  examine  the  deviation  of 
the  discrete  time  solution  (finite  element  method)  from  the  continuous 
case,  and  to  explore  the  feasibility  of  a  real-time  minimum  time 
controller. 

In  Chapter  2,  the  minimum  time  control  problem  is  stated.  The 
basic  concepts  of  control  theory,  as  well  as  some  variational  calculus 
principles,    utilized   in  the  problem  formulation,    are  presented. 

In  Chapter  3,  the  mathematical  model  of  the  r-theta  manipulator  (a 
two  degree  of  motion  manipulator)  is  developed.  The  dynamic 
equations, derived  using  the  Lagrangian  formulation,  are  used  in  the 
control   algorithm  for   the  minimum  time    simulation  of   the  manipulator. 

In  Chapter  4,  the  finite-element  solution  technique  for  the 
minimum  time  problem   is   developed. 
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In  Chapter  5,  the  simulation  results  are  presented.  The  finite 
element  method  is  found  to  converge  to  the  solution  with  reasonable 
initial  guesses  on  unknown  parameters.  When  this  method  is  used  in 
conjunction  with  a  grid  search  method  to  start  the  algorithm,  it 
converges  quite  rapidly  to  the  true  solution.  The  discrete  time  solution 
compares  favorably  with  the  continuous  case,  and  as  the  grid  density  of 
the  finite  element  mesh  is  increased  the  accuracy  of  the  solution  is 
improved. 

Some  of  the  limitations  of  the  technique,  as  well  as 
recommendations   on  areas   for   further   investigations   are    also  presented. 
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CHAPTER  2 
THE  MINIMUM  TIME  CONTROL  PROBLEM 

Optimal  Control  Theory 

The  objective  of  optimal  control  theory  is  to  determine  the 
control  signals  that  will  cause  a  process  to  satisfy  the  physical 
constraints  and  at  the  same  time  minimize  (or  maximize)  some  performance 
criterion. 

In  order  to  evaluate  the  performance  of  a  system  quantitatively, 
the  designer  has  to  select  a  performance  index  or  cost  function  J.  An 
optimal  control  is  defined  as  one  that  minimizes  (or  maximizes)  the 
performance  index. 

In  the  general  case,  it  will  be  assumed  that  the  performance  of  a 
system  is  evaluated  by  a  measure  of  the  form  [12] 

J  =  h(x(t^),t^)  +  /^*  g(x(t).u(t).t)  dt  (2.1) 

o 

where  t  is  the  initial  time,  t.  is  the  final  time,  and  h  and  g  are 
scalar  functions.  The  final  time  t.  may  be  specified  or  free  depending 
on  the  problem  statement.   For  the  minimum  time  problem 


t 

t 
o 


J  =  //  1  dt  (2.2) 
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where  t   is  unspecified. 

Throughoat  this  paper  bold  face  characters  will  represent  vectors. 
For  example  z(t)  and  u(t)  are  the  state  and  control  vectors. 

The  optimal  control  problem  is  to  find  an  admissible  control  which 
causes  the  system  described  by  the  set  of  first  order  ordinary 
differential   equations 

x(t)    =  •(x(t).n(t).t)  (2.3) 

to  follow  an  admissible  trajectory  z  (t)  that  minimizes  (or  maximizes) 

the  performance  index  J.  The  quantity  n  (t)  which  minimizes  J  is  called 

the  optimal  control  and  z  (t)  an  optimal  trajectory.   The  elements  of 

equation  (2.3)  are  called  state  equations  and  involve  the  state 

variables  z(t)  and  the  controls  ii(t).   A  more  formal  definition  of  state 

variables  is  provided  in  Chapter  3. 

A  control  history  which  satisfies  the  control  constraints  during 

the  entire  time  interval  [t  ,t.]  and  achieves  the  desired  final  state 

o   f 

z(t.)  is  called  an  admissible  control.   A  state  trajectory  which 

satisfies  the  state  variable  constraints,  both  the  differential  equation 

constraints  as  well  as  the  boundary  constraints,  during  the  entire  time 

interval  [t  ,t,]  is  called  an  admissible  trajectory. 
0   1  ^  J 

Variational  Formulation 
Variational  calculus  is  a  branch  of  mathematics  that  is  very 
useful  in  solving  optimization  problems.  The  performance  index  J  is  a 
functional.  A  functional  is  a  function  of  a  function  and/or  functions. 
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For  example,  if 


x^=  fjCq^-qj^  (2.4.1) 


and                                                        X2=  ^2^h''^2^  (2.4.2) 

where   <!.  >4,,   ai'e    independant   variables   and   £^ ,  f.    are   scalar   functions 

then  the   quantity                             J  =  g(x   ,x. )  (2.4.3) 
is   a   functional  where   g   is   a   scalar   function. 


Variation  of   a  Functional 

The    variation   of    a    functional  plays   the   same   role    in  determining 

extreme    values    (maximum   or   minimum)    of    a    functional    as    does    the 

differential     in    finding    maxima    or    minima    of    functions.       The 

differential,    df,    of  a   function,    f,    of  variables   q,>qo'**>'4      is    given 

1      z  n 


by  the   relation 


df  =  ^  dq,    +  ^  dq-    +    ...    +  ^  dq   .  (2.5) 

1^2  .  n 


aq^  aqj  aq^ 


Similarly,    the    variation,     SJ,     of    a    functional,    J,     of    functions 

X. ,x_,...,x   is   given  by  the   relation 
1     Z  n 

5J  =  "  &x,    +  ^-^  8x,+   ...   +  "  8x    .  (2.6) 

3  ^  a  •^  a  '^ 

OX,  OX-  OX 

12  n 


Fundamental  Theorem  of  the  Calculus  of  Variations 
The  fundamental  theorem  states  that  the  variation  must  be  zero  on 
an  extremal  (maximum  or  minimum)  curve,  provided  there  are  no  bounds 
imposed  on  the  curves. 
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In  other  words 


6J(x*.8x)  =  0  (2.7) 


for  all  admissible  6x.  By  admissible  5x,  it  is  meant  that  x  +  8x  must 
be  of  some  class  H  to  which  x  belongs.  For  example,  if  Q  is  the  class 
of  continuous  functions,  x  and  6x  must  both  be  continuous.  In  this 
case  Q  comprises  all  the  state  histories  which  satisfy  (2.3). 

Constrained  Minimization  of  Functionals 
So  far,  functionals  involving  the  state  vector  x(t)  have  been 
discussed  and  it  has  been  assumed  that  the  components  of  x(t)  are 
independent.  This  is  usually  not  the  case  in  control  problems  where  the 
state  trajectory  is  determined  by  the  control  u(t)  and  the  state 
equations.  Therefore  it  is  necessary  to  consider  functionals  of  n+m 
functions,  x(t)  and  a(t),  but  only  m  of  the  functions  are  independent  - 
the  controls.  The  next  step  is  to  derive  the  necessary  conditions  for 
extremals  of  constrained  systems.  The  Lagrangian  multiplier  method  will 
be  used. 


The  Lagrangian  Multiplier  Method  for  a  System 
with  Differential  Equation  Constraints 

The  objective  is  to  find  the  necessary  conditions  for  functions 

*        * 
X  (t)  and  n  (t)  to  be  extremals  for  a  functional 

J(x,u)  =  /^   g(x(t),u(t),t)  dt  (2.8) 

o 

where  x  (t)  is  the  state  vector  of  order  n  and  n  (t)  is  the  control 
vector  of  order  m.   These  vectors  must  also  satisfy  equation  (2.3),  the 


-10- 


differential  equation  constraints  on  the  states.  To  include  these 
constraints  the  augmented  functional  is  formed  .  The  augmented 
functional    is   defined  as 

J    (x.u.X)   =  //{g(x(t).u(t),t)+  X^(t)[a(x(t),u(t).t)-x(t)])dt    (2.9) 
a  t 

o 

where  X.(t)    i  =  1,2, ...,n  are   the  Lagrangian   multipliers   whose    values 

are    to   be    determined.   ^Then  the   constraints   are   satisfied,    the   augmented 

functional,     J     ,     equals     the     functional,     J,     for    any     X(t). 
a 

The   quantity  g    (x(t) ,x(t) ,u(t) ,X(t) , t)can  be   defined  as 

a 

g  (x(t),x(t).m(t),X(t).t)  =  g(x(t),ii(t),t) 

+  X^(t)[«(x(t),u(t).t)-x(t)l     (2.10) 
so  that 

J  (x(t),x(t),tt(t),X(t).t)  =  fit    {g  (x(t),x(t).ii(t).X(t),t)}dt.(2.11) 
a  t    a 

o 

The  variation  of  the  functional  J  ,  SJ   , after  integrating  by  parts  and 

a    a 

simplifying  is 

6J^  =  [fia(x(t^),x(t^),u(t^),X(t^).t^)]^5xj+  [g^(x(t j) ,x(t^). 
dx 

n(t^).X(t^),t^)  -  [lia(x(t^),x(t^),ii(t^),X(t^),t^)]^x(t^)l8t^ 

dx 


+  /^^{[[fia(x(t).x(t).u(t).X(t),t)]' 
o   dz 


-  f_[fia(x(t),x(t),u(t),X(t),t)]'''l  8x(t) 
dt  dx 
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+  [^a(x(t).x(t),u(t),A.(t).t)]^  8tt(t) 
dn 

+  [fia(x(t),i(t).tt(t).X(t).t)]^  8>.(t)}  dt.  (2.12) 

dX 

The  necessary  conditions  can  be  derived  from  the  above  equation  by 

applying  the  fundamental  theorem.   However  it  is  more  convenient  to  use 

another  functional,  the  Hamiltonian,  which  can  be  defined  as  [12] 

H(x(t).n(t).X(t).t)  =  g(x(t).u(t),t)  (2.13) 

+  X^(t)[»(x(t).n(t).t)l 
where   a(x(t) ,ii(t) , t)    is   the   right  hand   side   of  equation    (2.3). 

For  the  minimum  time  problem, the   Hamiltonian  can  be  written  as 

H  =   1  +  X^(t)[a(x(t).ii(t).t)l.  (2.14) 

For    an    extremal    curve    the    fundamental    theorem    gives    us    the 
condition 

5J    (x   (t).ii*(t).A.*(t).t)    =  0.  (2.15) 

a 

The  superscript  •  signifies  the  extremal  or  optimal  value. 

The  above  equation  gives  us  the  necessary  but  not  the  sufficient 

conditions  for  optimal  control  which  are 

x(t)  =  ff  (x*(t).u*(t).X*(t),t).  (2.16) 

dX 

XU)   =  -f^  (x*(t).u*(t),X*(t),t).  (2.17) 

dx 

dE        *  *  * 

_  (x  (t).ii  (t),X  (t).t)  =  0  .  (2.18) 

da 
and       0  =  [-  X*(t^)l''^6x^  +  [H(x*(t^),u*(t^),X*(t^). t^)l6t^  .   (2.19) 
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Equation  (2.16)  constitutes  the  n  state  equations.  Equation  (2.17) 
constitutes  the  n  co-state  equations.  Equation  (2.18)  constitutes  the 
m  optimality  conditions.  Equation  (2.19)  constitutes  the  boundary 
condition  equation.  The  conditions  above  are  not  sufficient  to  solve 
the  minimum  time  problem  because  constraints  on  the  controls  are 
required  to  solve  the  problem.  If  the  controls  are  unconstrained  then 
the  optimal  control  will  be  infinte  torque  or  infinite  force  and  the 
minimum  time  will  be  zero.  The  control  constraints  are  defined  in 
Chapter  3.   For  the  minimum  time  problem  the  final  time,  t.,  is  free  to 

vary,  but  the  final  state  x   is  fixed.  Therefore 

8x^  =  0.  (2.20) 

The  boundary  condition  equation  reduces  to 

H(x  (t^).u  (tj).X*(t^).t^)  =  0.  (2.21) 

This  equation  is  also  called  the  transversality  equation. 

So  far  it  has  been  assumed  that  the  admissible  controls  and  states 
are  not  constrained  by  any  boundaries,  however,  in  realistic  systems 
such  constraints  do  commonly  occur.  Physically  realizable  controls 
generally  have  magnitude  limitations.  Actuators  in  robot  joints  have  a 
maximum  torque  output  beyond  which  they  saturate.  The  generalization  of 
the  fundamental  theorem  to  include  the  effects  of  the  control  boundary 
constraints  leads  to  Pontryagin's  minimum  principle. 

Pont rya gin's  ■iniaaa  principle  states  that  an  optimal  control 
must  minimize  the  Harailtonian,  i.e. 

e(x  (t),ii*(t),3l*(t),t)  =  H(x*(t),n(t).X*(t).t)       (2.22) 
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for  any  t  e  [t  >t.]  and  for  all  admissible  controls, 
of 

The  conditions  for  minimum  time  control,  equations  (2,16),  (2,17), 

(2,18),  (2,21)  and  (2.22)  are  utilized  in  the  continuous  time  simulation 

and  in  the  discrete  time  simulation  of  a  robotic  manipulator  in  Chapters 

3  and  4,  respectively. 
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CHAPTER  3 

MATHEMATICAL  MODEL 

The   Concept   of   State 

The   concept   of   state   occupies   a   central  position   in  modern  control 

theory.       It    is    a    complete    summary    of    the    status    of    the    system   at    a 

particular    point    in    time.      Knowledge   of  the    state   at    some    initial   time 

t    ,     plus    knowledge    of    the    system    inputs    after    t     ,     allows    the 
o  0 

determination  of   the    state   at    some    later   time   t    .      At   any  fixed   time    the 

state  of  a   system  can  be   described  by  the  values   of    a    set    of    variables 
z.,        i    =    1,    2,     ...    ,    n   where    n    is    the    order    of    the    system.      These 

variables   are   called   the    state  variables. 

The  Mathematical  Model 

An   important  part   of   any  control  problem   is  modelling   the  process. 

The    objective    is    to    obtain    the    simplest    mathematical    model    that 

adequately   predicts    the    response    of    the    physical     system    to    all 

anticipated    inputs.    The    r-theta    manipulator   belongs   to   the   class   of 

systems   that   can  be    described   by    ordinary   differential    equations    in 

state    variable    form.       Thus    if    x,(t),x_(t),    ,x    (t)    are    the    state 

12  n 

variables   of   the  process   at    time    t    and   u,  ( t ),  u«  (t ),...,  u    (t)    are    the 

12  m 
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control     inputs    to    the    process    at    time    t,    then    the    system  may   be 
described  by  n  first   order  differential   equations,    such  as 

i,(t)    =  a,(x,(t).x-(t),....x    (t),u,(t),u-(t),...u    (t)) 
i.  iiz  nxz  m 


i-(t)   =  a,(x,(t),x,(t)....,x    (t),u,(t),n,(t),...n   (t)) 


.    (3.1) 


i    (t)    =  a    (x,(t),x-(t),...,x    {t),u,(t),u-(t),. 
n  n     1  z  n  1  z 

The  state  vector  x(t)  of  the  system  is  defined  as 


.u  (t)) 
m 


dt)   = 


and  the  control  vector  is  defined  as 


i(t)  = 


x^(t) 
X2(t) 

i*(t) 
n 


u  (t) 
uj(t) 

u*(t) 
m 


The  state  equations  in  vector  form  are 


(3.2) 


(3.4) 


x(t)    =  B(x(t),n(t).t) 


(3.4) 


Kinematic  Model 
The    two    de grees-of-f reedom   robotic    manipulator    on   which    the 
minimum  time   control    is  performed    is   called   an  r-theta    manipulator.      A 
schematic    of    the    manipulator    is    shown    in   Figure    3.1.      The   kinematic 
model    is    illustrated    in  Figure    3.2. 
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The    r-theta    manipulator  has   two  joints.      Joint   1    is   revolute   and 
joint  2    is  prismatic.      The   joint    variables    are    6    and    r    (or   D.     in    the 

Denav  i  t-Ha  r t enberg  representation  [13]  used  in  Figure  3.2), 
respectively.  The  torque  on  joint  1  is  T  and  the  force  on  joint  2  is  F. 
The  plane  of  motion  of  the  manipulator  is  parallel  to  the  ground  and 
hence   the   gravitational   force   does  not   enter    into   the   dynamic   equations. 

Dynamic  Model 
The   equations  of  motion  are   nonlinear    and    coupled.       In   order    to 
simplify    the    equations,     and    hence    the    simulation,    the    following 
assumptions   are  made. 

1.  The   mass    of    the    payload    is    much  greater   than  the  mass  of   the 
links   and   actuators. 

2.  The  payload  m   is   treated  as   a  point  mass. 

The  first  assumption  allows  the  r-theta  links  to  be  treated  as  massless 
kinematic  linkages.  The  second  assumption  simplifies  the  inertia  terms. 
The  dynamic  equations  are  derived  from  Lagrange's  equation  of 
motion.  If  not  all  the  forces  acting  on  the  system  are  derivable  from  a 
potential,    then  Lagrange's   equations   can  be  written   in   the   form    [14] 


d_(ai^)  _(aL_)  ^  Q  (3  5j 

dtOq.)   Oq.)     •' 
J       J 

where  L  is  the  Lagrangian,  q.  represents  the  generalized  coordinates, 
and  Q.  represents  the  forces  not  arising  from  a  potential. 


-19- 


If  K£  is  the  kinetic  energy  and  PE  the  potential  energy  then  the 
Lagrangian  is  defined  as  [14] 

L  =  KE  -  PE  .  (3.6) 

Soiniaing  up  the  kinetic  energy  of  the  manipulator  gives 

KE  =  ^  n(r)^  +  1  m(re)^  (3.7) 

2         2 

■ 

where  m  is  the  mass  of  payload,  9  is  the  joint  1  variable,  9  is  the  time 

derivative  of  9  ,  namely  (9),  r  is  the  joint  2  variable,  and  r  is  the 

dt 

time  derivative  of  r  ,  namely  (r).   The  potential  energy  of  the 

dt 

manipulator  is 

PE  =  0  .  (3.8) 


The  quantities  r  and  9  are  explicit  functions  of  time,  t. 
Throughout  this  chapter  the  independant  variable  t  is  omitted  from  the 
notation  of  the  explicit  functions  of  t  for  brevity.   Also,  throughout 

this  chapter  the  superscript  *  will  indicate  the  first  differential  with 

d 
respect  to  time   and  the  superscript     will  indicate  the  second 

dt 

2 
2 


differential  with  respect  to  tine  


dt 
The  forces  at   the  joints  are 

Q^=  T  (3.9) 

»nd  0^=  F  (3.10) 
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where  T  is  the  torque  at  joint  1  and  F  is  the  force  at  joint  2. 
Substituting  equations  (3.7)  and  (3.8)  in  equation  (3.6)  gives  the 
Lagrangian 

L  =  _  m(r  +  rV)  .  (3.11) 

2 

For    the    r-theta    manipulator,     the     independent    generalized 

coordinates    are    r    and   9.      The    Lagrangian    equations    for    the    r-theta 

manipulator   are   of   the   form 

d_(3L)   jaL)    ^  P  (3.12) 

dtOr)      Or) 

and  L^'l^  -^'±'   =  T   .  (3.13) 

dtoe)     (^«) 

From  equation  (3.11)  the  expressions  for  the  various  derivative  terms  of 
equations  (3.12)  and  (3.13)  are 

fi  =  mre^    .  (3.14) 

dr 

d    OL)        d    (mr)  ••  ,,   ,,, 
=  =  mr   ,  (3.15) 

dtOf)        dt 

!!l  =  0    .  (3.16) 

ae 

.                                     d    OL)        d    (mr^e)  2  "      ^  ,      . '  ,,    ,.,> 

and  =  =mr9+  ZmrrS   .  (3.17) 

dtoe)      ^^ 

Substituting  equations  (3.14)  and  (3.15)  into  equation  (3.12)  gives 

'2 
m  r  -  mre  =  F  .  (3.18) 

Substituting  equations  (3.16)  and  (3.17)  into  equation  (3.13)  gives 
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2  " 
mr  e   +  2mrre  =  T  .  (3.19) 

The  controls  are  defined  as 

Uj  =  ^  (3.20) 

Fmaz 

«nd  u^  =  ^  (3.21) 

Tmax 

where  u   is  the  control  at  joint  2,   u   is  the  control  at  joint  1,  T  is 

the  actual  torque  applied  at  joint  1,  Tmax  is  the  maximum  torque  that 
can  be  applied  at  joint  1,  F  is  the  actual  force  applied  at  joint  2,  and 
Fmax  is  the  maximum  force  that  can  be  applied  at  joint  2.  The  controls 
u.  and  u_  are  explicit  functions  of  t. 

Substituting  equations  (3.21)  and  (3.20)  into  equations  (3.19)  and 
(3.18),  respectively,  gives 

2" 
mr  e  +  2mrre  -  Tmax  u,  ~  °  (3.22) 

*2 
and  mr     -  rare     -  Fmax  u     =  0.  (3.23) 

The  state  variables  x.,  x.,  x. ,  and  x  are  now  introduced.  They  are 
defined  as 

^^  =  r,  (3.24.1) 


^2   =   r,  (3.24.2) 


X3   =  e,  (3.24.3) 


aiid  ^4  =  ®   •  (3.24.4) 
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From  equations  (3 .22 )  ,  (3 .23 )  and  (3.24)  the  state  equations  can  be 
written  as 

ij  =  x^.  (3.25.1) 

ij  =  x^xj   +!!:!!u^      .  (3.25.2) 

m 

i,  =  x^.  (3.25.3) 

3  4 

2x_x.    .    Tfltax  ,-   -,    .. 

and  X.  =  -24  +  u.  (3.25.4) 

4  r     2 

1  mx 

where   x   ,    x. ,    x,,    x.    are   explicit   functions   of   t. 

In   this    paper    the    performance    index   J      is   formulated   in  2  ways 

a 

which  are    : 

1.  The  First  Order  Foranlation  where  the  performance  index  J  is 
augmented  with  first  order  ordinary  differential  equation 
state  constraints  (refer  to  equation  (2.9)).  This  formulation 
is  used  in  the  continuous  time  simulation  (numerically 
integrated  using  the  Runge-Kutta  method). 

2.  The  Second  Order  Fomalation  where  the  performance  index  J  is 
augmented  with  second  order  ordinary  differential  equation 
state  constraints.  This  formulation  is  used  in  the  discrete 
time  simulation  (using  Finite  Element  methods). 

The  First  Order  Formulation 

From  equations  (2.9)  and  (3.25)  the  augmented  performance  index  J 

can  be  written  as 
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1 ,        2     Fmai 


J^(x.i.ii.X)=  //    {1+  k^U^  -  ij]    +  ^2^Xl^4*  ^^^  ""l     "  ^2^ 


a 


+  xhx,   -  i,]    +  \][-'!V4.  +  ^  u-   -  xJ}dt  (3.26) 

3      4  3  4  ,2  4 

1  mz. 

where  the  superscript  1  on  the  X's  signifies  first  order  formulation  and 

1111 
where  X.,  X- ,  X.,    X.    are  the  Lagrangian  multipliers  which  are  explicit 

functions  of  t. 

From  equations  (2.14)  and  (3.25)  the  Uamiltonian  for  the  r-theta 
manipulator  can  be  defined  as 

1      1    ,   P  1 

H(x.ii,X)  =  1  +  X^x^  +  >-2l»i*4  +    ''  ^i   ^  +  5^3% 

m 

.  .  r   2x_x.   Tmax    ,  /«  «»» 

+  X,[-   2  4  +  u,  ]   .  (3.27) 

1      mx 

Substituting    equation    (3.27)    in   equation    (2.17)    gives    the    co-state 
equations   for   the   r-theta  manipulator  as 

X     =  -  X,x2  -  X.[!!2^-  51^  u,      ].  (3.28) 

1  2   4  4        2  3      2 

.1  11 

Xj  =  -  X^  -  X^[-l^]    ,  (3.29) 

*1 

.1 

X^   =  0   ,  (3.30) 
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.11  11  -2x 

and  5l .  =  -  X-[2x,x.]  -  X,  -  X .  [  _  ]   .  (3.31) 

4      2    14      3     4 

^1 


Sabstituting  equation  (3.27)  into  equation  (2.21)  gives  the 
transversality  condition  for  the  r-theta  manipulator  as 

«   ,  .  ,     .  ,  r   2  ,  Fmax    , 
0=1+  X^x^  +  X2[x^x^  + u^  ] 

m 

.  ,1    ,  ,l.(-2x-x.)  Tmax    ,           /»-.«» 

+  X,x.  +  X. [    2  4   +  u-  ]  (3.32) 

3  4     4  _   2 

X,  2 

1  mx 

at  t  =  t^. 

For    the    simulation   example    it    is    assumed    that  the    manipulator 

starts   from  rest   and  come   to   a   stop   at   the   final    state.  Therefore,    the 
state  variables 

X2(t^)   =  0  (3.33) 

and  X. (t.)   =  0   .  (3.34) 

4      f 

Substituting  equations  (3.33)  and  (3.34)  into  equation  (3.32)  gives 

1  +  X-[!^  u,  ]  +  X^!!!f  u,  ]  =  0.  (3.35) 

mx^ 

The  Second  Order  Formulation 

The  augmented  functional  J   is  defined  in  terms  of  the  second 

a 

order  differential  equation  constraints  (3.22)  and  (3.23)  as 
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J    (x.x.u.X.t)   =  /      (1+  X, [Fmax  u,    -  mr  +  mrG   ] 

t  ^  ^ 

o 

+  X,[Tinax  u,   -  f_(mr^e)l}    dt  (3.36) 

^  ^        dt 

where  the  X's  with  no  superscripts  signify  the  second  order  formulation. 

Taking  the  variation  J   and  applying  the  fundamental  theorem  gives 

the  two  multiplier  equations  and  the  transversality  equation  (refer  to 
Appendix  I).  The  multiplier  equations  are  the  second  order  differential 
equations  in  the  Lagrangian  multipliers.  They  are  given  by  the 
following   relations 

-  m  X     +  mX  e     +  X,2mre  =  0  (3.37) 

and  2m[Xj{fe  +   r  9   }    +  k  t9]    +  X^mr     +  X  2mrf  =  0    .  (3.38) 

When  the  velocities  at  the  final  state  are  zero  the  transversality 
equation   is 

1  +  X^[Fmax  u^   ]    +  X^ETmax  u^   ]    =  0  (3.39) 


which   is   eqvivalent   to   the   first   order   formulation   (equation   (3.35)). 

The  Optimality  Conditions   for   a  Problem  with 
Inequality  Constraints 

Figure    3.3    [2]    provides    one-dimensional    illustrations    of    two 

possible   types   of  minima  with   inequality  constraints.      It    is   required   to 

minimize   a   functional   Ku)    subject   to   the    inequality  constraints 

f(«)    <  0  (3.40) 

-26- 


3 


3 

A 


■"1        3 


0) 

•H 
CO 

M  . 
O  CO 
O.  4-1 

c 

O    -H 

>  cd 

4-1     Vl 
4-1 

<4-l     CO 

o  c 
o 


CO 


4J 


3 

3  U" 

iH  0) 

^  C 

•H  -H 

rH  ^ 

CO  4J 

C  -H 

O  S 

•H 

to 

c  2 

e  -H 

•H  C 

I  e 

<u 

o  o 

CO 

•  a) 
en  o. 

•  >. 
m  4-1 

<u 

3 
60 


-27- 


where  in  general  f  and  n  are  vectors  of  different  dimensions.   The 
constraints  can  be  appended  to  the  functional  I(u)  to  give  the  augmented 

functional  I 

a 

I  iu.n)   =   I(u)  +  |if(ii)  (3.41) 

a 

where   |i   is   the  vector  of  the    inequality   constraint    multipliers.      Y/hen 

these  constraints  are  satisfied 

I  (n.^)  =  Kb)  .  (3.42) 

a 

* 
There  are  two  cases  for  the  optimal  value  of  v.  n  ,  which  are 

f(u*)  <  0  (3.43) 

and  £(«*)   =  0    .  (3.44) 

In  the   former  case   (i  =  0   so   that   equation   (3.42)    is    satisfied.       In    the 

latter    case    consider    small    perturbations    about    v    .       If    Ku    )     is    a 


minimum,    then 


51  =  f^  6u  >   0  (3.45) 

du 

for  all  admissible  values  of  5n,  which  must  also  satisfy 

8f  =  ££  8n  <  0  .  (3.46) 

da 

For  equations  (3.45)  and  (3.46)  to  be  true  they  must  be  of  opposite 

sign  which  indicates 

sgn  (")  =  -sgn  (^)  (3.47) 

dn  du 

or  ^  =  0  (3.48) 

du 

where  the  signum  function,  sgn,  is  the  sign  of  the  argument  and 
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31                                   ^^1   ^^2        ^^ 
where  ( )  is  the  negative  linear  combination  of  , 


»  •  • •  #, 


du  du  du  du 

where  m  is  the  number  of  constraints.   Equations  (3.47)  and  (3.48)  can 
be  combined  to  give 

!!+|i!!=0  (3.49) 

dn    du 

for  |i  2  0.  (3.50) 

Therefore  the  necessary  conditions  for  minimizing  I(n)  are 

(3.51) 

(3.52) 
(3.53.1) 
(3.53.2) 
(3.53.3) 
(3.53.4) 

For  minimum  time  problems  bang-bang  control  is  used,  that  is 

»  =  +  u  (3.54) 

—  max 


*  =  0 
dn 

and 

f(n)  i  0 

subject  to  the  conditions 

|i  >  0 

for 

f(ii)  =  0 

and 

p  =  0 

for 

f(ii)  <  0. 

where  u  is  the  control  vector  and  a    is  the  maximum  control  magnitude. 

max  " 

For  the  r-theta  manipulator 

n    =  1  .  (3.55) 

max 

From    equations    (3  .52 ) , ( 3 . 54 )     and    (3.55)     the     inequality    control 
constraints   for   the   r-theta  manipulator   are 

fjCa^)    =  u^  -  1    .  (3.56.1) 
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f^Cu  )  =  -u^  -  1  ,  (3.56.2) 

f  (u^)  =  u  -  1   .  (3.56.3) 

and  f^(u,)  =  -u,  -  1  .  (3.56.4) 

4   2       2 

The  functional  I  for  the  first  order  formulation  is  equal  to  H  in 

equation  (3.27) 

1      1    ,   P  1 

Kx.u.X)  =  1  +  X^i^  ■"  ^2^''l''4  ^  —-   "l   ^  ^   ^3% 

m 

+  X.t-  ^^2^  +  !!!!  u,  ]  .  (3.57) 

1      mx^ 

From  equations  (3.41)  and  (3.57)  I   is 

a 

I^(x,n,X,|i,t)  =  I  +  n^(Uj-l)  +  ii^i-vi^-1) 

+  li^in^-1)    +  ^^(-u^-l)  .  (3.58) 

Substituting  equation  (3.58)  into  equation  (3.51)  gives 


du^        n 


31  1-. 

and                   =  X     "'  ^  +  (1  -n  =  0                (3.60) 

3u-  ^   2    "*  ^ 

2  mx. 

subject  to  the  conditions  (3.52),  and  (3.53).   From  equation  (3.59) 

if  ''l  "  ^                       (3.61.1) 

^^^^  Pj  2  0                       (3.61.2) 
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and  ^     =  0    .  (3.61.3) 


TTierefore                                                      K  ^  ^  (3.61.4) 

in  order  for  equation  (3.59)  to  be  true.  If 

u^  =  -1  (3.62.1) 

then                          H  =  0  (3.62.2) 

and                           Hj  -  °  •  (3.62.3) 


Therefore  ^2  -  ^  (3.62.4) 

in  order  for  equation  (3.59)  to  be  true.   From  equations  (3.61)  and 
(3.62)  the  control  u  can  be  defined  as 

U  I      1 

u  =  -  =  -sgn(A.  )  (3.63) 

^1         ^ 


for                            \^  ^  0    .  (3.64) 

The  signum  function,  sgn,  takes  on  a  value  which  is  equal  to  the  sign  of 

the  argument.   Similarily  from  equation  (3.60)  the  control  u.  is  defined 

as 


U  I      1 

u,  =  -  =  -sgn(X.)  (3.65) 

^       1         * 
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for  X.  /  0  .  (3.66) 

4 

1      1 
When  X_or  X.  is  zero  the  respective  control  can  move  away  from 


the  constraint  boundary.   However  for  the  r-theta  manipulator  X     and  X. 

are  at  zero  for  only  an  instant  and  therefore  the  effect  of  control  at 
those  instances  is  not  significant. 

For  the  second  order  formulation  the  control  a   is  defined  as 


U  I 
u  =  -      =  -sgnO.  )  (3.67) 

X 


for  X^  *  0  (3.68) 


and  the  control  a.  is  defined  as 


\X    \ 

Uj  =  -  =  -sgn(A.2)  (3.69) 

^2 


for  X^  ?*  0  .  (3.70) 

These  optimality  conditions  are  used  in  the  continuous  time  and  discrete 
time  simulations. 
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CHAPTER  4 

THE  FINITE  ELEJIENT  METHOD 

Introdaction 
In  Chapter  3  the  mathematical  model  of  the  manipulator  was 
formulated.  The  state  equations,  the  multiplier  equations,  the 
transversality  equation,  and  the  optimality  conditions  were  derived  for 
the  r-theta  manipulator  by  both  the  first  and  the  second  order 
formulations.  The  first  order  formulation  is  used  in  the  continuous 
time  simulation  where  the  set  of  eight  first  order  differential 
equations  are  numerically  integrated  using  the  Runge-Kutta  method.  The 
guesses  on  the  unknown  parameters  (the  Lagrangian  multipliers  at  the 
initial    time    t       and    the    final    time    t.)     are    iterated    upon   using 

o  I  "^  " 

conventional  minimization  techniques  like  the  conjugate  gradient  method 
[15],  the  Quasi-Newton  method  [16],  and  the  Newton-Raphson  method  [17]. 
However  none  of  the  methods  converge  to  the  solution  if  the  initial 
guesses  on  the  unknown  parameters  are  not  sufficiently  close  to  the 
optimal  values. 

A  more  robust  method  of  solving  the  minimum  time  problem,  a  two 
point  boundary  value  problem  (TPBVP),  is  needed.  Both  the  optimal 
control  problem  and  the  finite  element  method  can  be  developed  from 
variational  principles.  The  finite  element  method  has  been  successfully 
applied  to  a  wide  range  of  nonlinear  problems  as  well  as  to  the  TPBVP. 
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Therefore,  this  method  was  applied  to  the  solation  of  the  minimua  time 
problem  with  the  objective  of  investigating  the  advantages  and  drawbacks 
of  a  discrete  time  simulation  as  compared  to  the  continuous  case.  A 
flow  chart   of   the   finite   element  program  is    included   in  Appendix  II. 

Finite  Element   Analysis 

The  finite  element  method  is  a  numerical  analysis  technique  for 
obtaining  approximate  solutions  to  a  wide  range  of  engineering  problems. 
In  nonlinear  problems,  as  in  this  particular  case,  closed  form  solutions 
are  not  available,  so  it  is  necessary  to  obtain  approximate  numerical 
solutions.  Two  of  the  more  commonly  used  methods  are  the  finite 
difference  and  the  finite  element  methods  [18].  For  some  problems, 
especially  problems  with  irregular  geometry  or  unusual  boundary 
conditions,  the  finite  element  method  is  superior  to  the  finite 
difference  method. 

The  finite  element  method  takes  a  continuum  problem  and 
discretizes  the  solution  region  into  a  finite  number  of  elements.  By 
expressing  the  unknown  solution  within  each  elenent  in  terms  of  assumed 
approximating  functions  called  interpolation  functions,  the  infinite 
number  of  unknowns  in  terms  of  the  Taylor  series  expansion  is  reduced  to 
a   finite   number. 

One  of  the  advantages  of  this  method  is  the  ability  to  formulate 
the  properties  of  the  individual  elements,  before  putting  them  together 
to  represent  the  entire  problem.  In  effect,  a  complex  problem  is 
reduced  to  considering  a  series  of  greatly  simplified  problems.  Another 
advantage    is   the  variety  of  ways   in  which  the  problem  can  be    formulated. 
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These  include  the  direct  approach  (from  physical  laws),  the  variational 
approach,  the  weighted  residual  approach,  and  the  energy  balance 
approach  [18].  In  this  paper  the  variational  approach  was  used  in 
determining   the   element  properties. 

There   are   5  basic    steps   to   the   finite   element  method.      These   are: 


1.  Discretization  of   the   continuum. 

2.  Selection  of   the    interpolation   function. 

3.  Determination  of   the   element  properties. 

4.  Assembly  of  element  properties    to  obtain  system  equations. 

5.  Application   of   boundary   conditions    and    solution   of    system 
unknowns . 

Discretization  of  the  Continuum 
The  first  step  is  the  discretization  of  the  solution  region  into 
elements.  The  range  of  the  independent  variable,  time  t,  from  the 
initial  state  to  the  final  state  is  discretized  into  elements  of  uniform 
length  At.  These  elements  are  connected  to  adjoining  elements  by 
sharing  common  nodes.  The  element  length.  At,  varies  with  change  in 
grid  density  or  the  final  time.  The  element  unknowns  are  the  position 
coordinates    (r,0)    and   the  Lagrangian  multipliers    (X   ,    k   )    at   each  node 

and    the    length    of    element    (At).       The    performance    index    J      will    be 

a 

expressed    in    terms    of    the    approximations    so   that    the   continuous   time 
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problem  of  minimizing  J      over   the    time    interval    [t    ,t.]     is    reduced    to 

a  of 

one  of  minimizing  J   for  each  element  in  the  time  domain. 

a 

Discretizing  equation  (3.35)  gives 

-.     n    riAt,,   ,  ._  *2    "  , 

J   =  snm  J        [1+  \    [Fmaz  u,   +  mrS  -  mr  ] 

*    i=l    (i-l)At  ^       ^ 

+  X-ETmax  u.  -  f_  (mr^G  ) ] }  dt       (4.1) 
^       ^    dt 

where  n  is  the  number  of  elements  and  At  is  the  length  of  each  element 

and  sum   is  the  summation  over  the  elements  1  to  n. 
i=l 


Selection  of  the  Interpolation  Function 
The  next  step  is  to  assign  nodes  to  each  element  (points  in  time) 
and  choose  the  interpolation  function  to  represent  the  variation  of  the 

unknown  variables  over  the  elements.   The  state  variables  r,  r,  9,  9 

and  the  multipliers  and  their  time  derivatives  X   X   .    k       X.  will  be 
represented  by  linear  interpolation  functions  of  the  form 

x(t)  =  N^(t)x^  '^^2^^'''^2    '  ^"^'^^ 

*2~^1 
and  x(t)  =  ^  (4.3) 

At 

where  x  ,  x.  are  the  values  of  the  given  unknowns  x(t)  at  nodes  1  and  2 

of  each  element  and  the  natural  coordinates  N  ,  N   vary  as  shown  in 
Figure  4.1 
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N 
1 


Noda  1 


Node  2 


>    t 


At 


Figure  4.1:   Natural  Coordinates  for  First  Order, 
One  Dimensional  Finite  Element 
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The  natural  coordinates  are  defined  by  the  relations 

N  =  1  -  ^  (4.4) 

At 

and  N  =  L.   .  (4.5) 

^        At 


Therefore, 


and 


/J^N^}  dt  =  /f  {N^}  dt  =  ^  .             (4.6) 

/q^N^N^}  dt  =  ^  .  (4.7) 
6 

&^\}fy    dt  =  /^^N^}  dt  =  ^  .             (4.8) 


The  relations  (4.6),  (4,7)  and  (4.8)  will  be  extensively  used  in  the 
development  of  the  element  equations. 

From  equations  (4.2)  and  (4.3)  the  unknown  variables  are  defined 


as 


r(t)  =  N^r^  +  N^r^  .  (4.9.1) 


e(t)  =  N^e^  +  N^e^  ,  (4.9.2) 

A.j(t)  =  NjX^^  +  N^X^^'  (4.9.3) 

^^(t)  =  N^X^^  +  N^X^j.  (4.9.4) 


Vl 
r(t)  = :  .  (4.9.5) 

At 


®2-®l 
e(t)  =  ^    ,  (4.9.6) 

At 


^12~^11 
^.(t)  =   ^^     .  (4.9.7) 

At 
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and  Lit)   =     ^^     ^^  (4.9.8) 

^  At 

where     X  and    X         are    the    values    of    X.(t)     at    nodes    1    and    2, 

respectively,    of  each  element  while   X       and  X..    are    the  values    of    X    (t) 
at     the     respective     node    designated    by    the    second    subscript. 


Determination  of   the  Element  Properties 
The    variational    approach  will    be   used    in   the    formulation  of   the 
element    properties.      The    equations    for    the    minimization    of    the 
performance    index  J      over    a    single    element   will   be    derived    in   this 

section.      In  the  next    section  the   elements  will  be   assembled   to   give   the 

equations    for    the    minimization    of   J      over    the    entire    time    period 

a 

J   in  equation  (4.1)  is  simplified  by  integrating  by  parts  the 

second  derivative  terms  in  the  equation.  This  gives  rise  to  two 
boundary  terms.  For  interior  elements  the  boundary  terms  cancell  with 
those  from  the  adjoining  elements.   For  exterior  elements  these  terms  go 

to  zero  because  r(0),  0(0),  f(t_)  and  9(t.)  are  specified  as  zero  in  the 

boundary  conditions.  Considering  equation  (4.1)  for  a  single  element 
and  substituting  equations  (4.6)  -  (4.9)  into  (4.1)  gives 
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6At  "  ^2 


2   1 
1  2 


pi 

L2_ 

1  -1 

[e^  e^] 

-1    1 

^ 

3_ 

m 


6At 


[r,  r^] 


At 


2  1 

1  2 

1  -1 

-1  1 


^1 

'2 

1  -1 

^hl   h2^ 

-1      1 

e. 


rAt, 


+  /.  {  X  Fmax  u  +  X  Tmax  u  1  dt  . 


(4.10) 
For  terms  containing  the  controls  two  situations  will  be  considered. 


Mo  switching  of  controls 

If  no  switch  (change  of  sign)  occurs  within  the  elenent,  then 

(4.11.1) 


and 


sgn(Xj^j^)  =  sgnCX^^) 


sgn(X2j^)  =  sgn(X^2) 


(4.11.2) 


are    true.      Substituting    equations    (3.63),    (4.6)    and    (4.9.3)    into   the 
first    integral   term   in  equation   (4.10)    gives 

/^*{X,Fmai  u,    }    dt  =  -  ^^  At    ( !    X,,    I    +    I    X,^    !  )    .        (4.13) 
oil  11  12 

Similarly  the  second  integral  term  in  equation  (4.10)  is  found  to  be 

/^X^Tmax  u^}  dt  =  -  I^  At  (I  ^21  '  "^  '  ^"22  '  ^  *    ^"^'^"^^ 
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Switching  of  controls 

If  a  switch  in  u   occurs  within  the  elenent  then 

sgnO.^^)  =  -  sgn(X^2)  •  (4.15) 

Let  t   be  the  time  to  switch  from  the  start  of  the  element  in  which  the 
s 

switching  occurs.   Then 

X, (t  )  =  0  (4.16) 

1   s 

or  N,(t  )X,,  +  N-(t  )>.,-  =  0  (4.17) 

1   S   11     L       S   IZ 

t       t 

or  (1  -  _!  )X,,  +  (^  )X,-  =  0  .  (4.18) 

At   ^^    At    ^^ 


Rearranging  the  above  equation  yields 

At  \ 


t  =  .  (4.19) 

^   \     -  X 


Therefore,  for  a  switch 

/^^{Xj^Fmax  u^  }  dt  =  -  /^^{X^Fmax  sgn(X^j)}  dt 

-  /^*^{X^Fmax  %%n.{\^^)^    dt  .      (4.20) 
s 

Substituting  equation  (4.19)  into  (4.20)  above  and  simplifying  gives 

2   ^  2 

/f  {X.Fmax  u.  }  dt  =  -^^^   sgn(X„)  At    ^^  ^  ^^   .    (4.21) 

2  (X,^  -  X^^) 

For  switch  in  u  within  the  element 

sgn(X2j^)  =  -  sgn(X22^  •  (4.22) 
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Following  the  same  steps  as  in  the  case  of  u   switch  gives 

t   =  (4.23) 

^21~  ^22 

2     2 

and       /Q*{^Tmax  u   1  dt  =  -'^^   sgnCX^.)  At    ^^    ^^   .    (4.24) 

2  (X^,  -  X^j) 


Element  equations  from  a  Variational  principle 

The  finite  element  solution  to  the  problem  involves  picking  the 
values  of  (t* .  (consisting  of  r,  0,  X.  ,  X. )  where  i  goes  from  1  to  p  and 

p  is  equal  to  four  times  the  niunber  of  nodes,  and  element  length  At,  so 

as  to  make  the  functional  J  ((j(.  At)  stationary.   To  make  J  ((b.  At) 

a  a 

stationary   with    respect    to    4> .    and    At    the     fundamental     theorem    of 

1 

variational  calculus  requires  that 

dJ                     p   dJ 
SJ  (*,At)  =  1  5At  +  sum  t   5(>.  =  0  .  (4.25) 

*         3At        i=l   a*.    ^ 

1 

Since  the  5^.'s  and  At  are  independant,  equation  (4.25)  can  hold  only  if 

3J 

_!.  =  0  (4.26.1) 

a<t>. 

1 

and  t  =  0    .  (4.26.2) 

dAt 

Therefore,    J      in  equation   (4.10)    is   differentiated   with    respect    to    the 

nodal  unknowns,    r   ,    r   ,    0   ,   0   ,    X.,,    X  .      X      ,    X  ^,    and  with  respect   to 
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time  At,  to  get  the  ejenent  equations.   Differentiating  J  with  respect 
to  r  ,  r  gives 


dJ 


m 


6At 


[e^  e^] 


1  -1 
-1    1_ 

«2 

2      1 
1     2 

hi 
hi 

3  At 


1  -1 
-1  1 


r«i 

«2 

_  — 

2   1 
1  2 


m 


At 


1  -1 
-1    1 

hi 
hi 

= 

0 
0 

(4.27) 


Let 


cml3  =  _^  (9  -  9  )^  , 
6At   -^   "^ 


(4.28) 


cnll  = 


3At 


^^1-V^hrhi^  ' 


(4.29) 


and 


cm31  = 


m 


At 


Substituting  equations  (4.28),  (4.29),  (4.30)  into  (4.27)  gives 


(4.30) 
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3J 

2 

1 

a 

=   cnlS 

1 

d 

^2 

2 

— 

_    _ 

11 


12 


+  cmll 


2  1 
1   2 


+  cm31 


1  -1 
-1  1 


11 


12 


(4.31) 


Let 


and 


m 


3At 


cm24  =  (r  +  r  r,  +  r   ) 

3At   ^    ^  ^    ^ 


(4.32) 


(4.33) 


Differentiating  J  with  respect  to  9  and  9  plus  substituting  cn22  and 

a  J.  ^ 

cm24  into  the  expression  gives 


3J 


a     =   cin22 


1  -1 
-1      1 


—    — 

\ 

^2 

+  cm24 


1  -1 
■1      1 


21 


22 


0 
0 


(4.34) 


Differentiating  J  with  respect  to  X,.and  X,-,  and  substituting  c:nl3  and 
a  11     12  ° 

cm31  into  the  expression  gives 
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5J 


a  =  cml3 


11 
^12 


2   1 
1   2 


+  cn31 


1  -1 
-1   1 


11 

^12 


/g^CX^Fnax  u^  }  dt  = 


(4.35) 


For  the    last   term  of  equation    (4.35)    there    exist    tv/o    situations 
depending   upon    the    occurance    of   a   switch   in  X    .      If   a   switch  does   not 

occur   then  \     does  not   change    sign.      Differentiating    equation    (4,13) 

with  respect   to  \     ,    \       gives 


i /^^{X^Fmax  u^   }    dt   = 


Fmax 


11 
^12 


At    sgn(X.^^  ) 


(4.36) 


If    a    switch   occurs    then   X      will    pass    through    zero.      Differentiating 
equation    (4.21)   with  respect   to  A.     and  X...    gives 


3   X 


/^^{X^Fraax  u^   }    dt  =  -  ^°°^  At    sgn(X.      ) 


11 


[. 


11 


ixl^  .  xl^) 


]    (4.37.1) 


^11-  ^12        2a^^-X^^)' 
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and 


a  X 


f      {X.Fmax  u     }    dt  =  -  "  At    sgn(X,.) 


12 


[. 


'12 


(X^     +  xj   ) 
_  +  ].(4.37.2) 

^ll"  ^12        2(X^^-/L^2^^ 

Differentiating  J     with  respect   to   X.,,    X_-    and   substituting    cm24    into 
a  21        22 

the   expression  gives 


3J 


a     =  cin24 


^22 


1  -1 

-1      1 

/■At, 


''^' 

«2 

_ 

+  ! /q    {X^Tnas  u^   }    dt  = 


^22 


0 
0 


.      (4.38) 


For  the   last   term   in  equation   (4.38)    there    exist    tv/o    situations 
depending   upon    the    occurrence   of   a   switch   in  X^ .      For   the    situation  of 

no    switch,    differentiating   equation    (4.14)   with   respect    to    X^^and      X^^ 

21  22 

gives 


fn    {X-Tmax  u.    }    dt  = 

rr     — ^     U         ^  z 

>^ 
^22 


Tnax 


At    sgn(X-.) 


(4.39) 


For   the    situation  where    a    switch  occurs,    differentiating   equation    (4.24) 
with  respect   to  X.    and     X       gives 
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! /^*{X,Tmax  u,  1  dt  =  -!!:!!  At  sgn(X,,) 


^21     ^^1  ^  ^22  > 
[_Ji_  -  _^^^ ]    (4.40.1) 

Sl~  ^2   2(X  -X  )^ 


and  t /J^X^Tmax  u^  )  dt  =  -  l!!f  At  sgn(;i2^) 

^  ^22  2 


,   ^2    J^21  ^  4)  ,  ,,  ,,   ,, 
[ +  ]. (4.40.2) 

^21-  ^22   2a^^-X^^)^ 


Derivation  of  the  transversality  equation. 

Differentiating  J   in  equation  (4.10)  with  respect  to  time  At  and 

simplifying  gives 

dJ 

!  =  1  -  ^[  cm22  f^  cml3  +  cm24  f^  cmll  +  cm31(r  -  t.)(X.-   X.,.)] 

aAt       At       m  m  ^   ^   ^^       ^^ 

+  /q  {A-jFmax  u^  +  X^Tmax  u^  }  dt  .  (4.41) 

3At 

There  are  two  situations  for  the  integral  terms.   If  no  switch  occurs, 

then 

!_  /^^X^Fmax  u^  }  dt  =  -^^   (I  X„  !  +  I  X,J)      (4.42) 
flAt  2  ^ 

and        i_  /o'{X  Tmax  n  1  dt  =  -"^   (I  X.J  +  I  X,  J )  .     (4.43) 
3At  2 

If  a  switch  on  u  occurs,  then 
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2     2 
f_  /^'{X^Fmax  u,  }  dt  =  -!!!!  sgn(X,,  )  ^^^^^^^  .    (4.44) 


If  a  switch  on  a.  occurs,  then 


2     2 

9    r^'fi  T       1  ^4.    Tmax    ,,   /*-21  "^  *'22^      ,.  ... 
J f.    iX-Tmax  u  1  dt  =  - sgn(X_  )  .    (4.45) 


The  nine  element  equations  derived  above,  (4.31),  (4.34),  (4.35), 
(4.38)  and  (4.41)  are  assembled  to  give  the  9x9  element  matrix  [A]  as 
shown  in  Figure  4.2.   The  nodal  unknowns,  r  ,  r. ,  9  ,  0  ,  X  ,  X  . ,  X_^, 

X..  ,and  At  constitute  the  vector  x.  The  element  sub-matrices,  M(i,j), 

are  two  by  two  matrices  for  values  of  i  and  j  ranging  from  one  to  four. 

The  submatrices  M(i,5)  are  two  by  one  vectors  while  the  submatrices 

M(5,i)  are  one  by  two  row  vectors.   The  submatrix  M(5,5)  is  a  scalar. 
The  element  equation  can  be  stated  as 

[  A  ]  X  =  f(x)  =  0  .  (4.46) 

To  solve  the  system  of  nonlinear  equations  the  Newton-Raphson 
method  will  be  used.   The  nonlinear  equations  are  linearized  by  using  a 

Taylor  series  expansion  about  the  true  solution  f(x  ).   Guesses  on  the 
unknown  variables  can  be  expressed  as 

x.=  x*+  Ax..  (4.47) 

where  i  is  the  iteration  number  and  Ax.  is  the  vector  of  deviations  from 

* 
the  solution  vector  x  . 
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Considering    the    first    two    terms    of    the    Taylor   series   expansion 


gives 


f   (X*   +  Ax^.    X*  +  Ax^. 


*  *        *  *  3f 

,x     +  Ax    )    =   f    (x, ,    x_, . . .,x   )    +  Ax, 

P  P  12  p  —  1 

axj 


+  ^^     Ax,+...+  ^'     Ax  (4.48) 

'k  2  T  p 

where    f    is    a    vector    of    order   p    for    a    system   of   p    equations.      From 
equation    (4.46) 


f(x)    =  0    . 
Therefore,    equation    (4.48)    reduces   to 


f (x)  =  !f  Ax  . 


dx 


(4.49) 


(4.50) 


The  Jacobian  [  J  ]  is  defined  as 


[  J  1  = 


3f 
ix 


bt        3f 
P    P 

3x^  TT^ 


Therefore  equation  (4.50)  can  be  written  as 

f(x)  =  [  J  ]  Ax  . 


ax,  ax^ 


af^  af^ 


af. 


dx 


af. 


dx 


af 


dx 


(4.51) 


(4.52) 
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Equation  (4.50)  is  solved  iteratively  and  the  guesses  are   updated  each 
iteration  until  the  convergence  criterion 

.2 


(Ax.y 


CONV  =  [  sum 


]°*^  <  l.OE-10 


(4.53) 


i=l   (  X.) 

1 


is  satisfied. 

Evualation  of  the  Jacobian 

The  Jacobian  is  a  9  x  9  matrix  that  is  defined  by  several 
sub-matrices  as  shown  in  Figure  4.3.  These  sub-matrices  are  presented 
in  the   following  development. 

From  equations  (4.46)  and  (4.51) 


eh 


J(i.i)  = 


=   cmll 


2^-1 


2   1 
1  2 


(4.54) 


and 


where 


Bh 


J(1.2)  = 


=  cjl2 


(1.3) 
(2.3) 


-(1.3) 
-(2.3) 


cjl2  = 


m 


3At 


and 


(1.3) 
(2.3) 


(4.55.1) 


(4.55.2) 


0.21"  hl^^^^l^   ^2^    ■*■  ^V  ®2^^^Sl'^  ^2^  '   ^'*-55-3) 
(>.2i~  ^2'^^!"^  ^'2^  *    ^®r  ®2^^'^ll'^  ^^12^  •   ('♦•55.4) 
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The  (1,3)  and  (1,4)  submatrices  are 


Bh 


J(1.3)  = 


=   cnlS 


dfl 


11 
^12 


2   1 
1   2 


+  cm31 


1  -1 
-1   1 


(4.56) 


and 


3h 


J(l,4)  = 


=  cjl4 


^22 


(1,7)   -(1.7) 
(2.7)   -(2,7) 


(4.57.1) 


where 


m 


and 


The  (1.5)  sabmatrix  is 


3h 


J(1.5)  = 


cji4  = (e  -  e  )  . 

3At   ^       ^ 


(1,7)  =  (2r^+  r^)   . 


(2,7)  =  (r^+  iT^) 


[  call 


aAt  a 


At 


2r^^  r^ 
V  2^2 


+  cml3 


(4.57.2) 

(4.57.3) 
(4.57.4) 


2^1^  ^12 


+  cm31 


^11~  ^12 
■^11^  ^2 


(4.58) 


Since  the  Jacobian  is  symmetric  we  have  the  condition 


,h 


J(2.1)  = 


=  [  J(l,2)  ]   . 


'l 

^2 

0 

Rl 

-  - 

__        ^ 

-53- 


(4.59) 


The  other  terms  from  the  second  row  of  subaatrices  in  the  Jacobian  are 


a^j 


J(2,2)  = 


cm22 


V 


1  -1 
-1  1 


(4.60) 


J(2,3)  = 


3^ 

J 

a 

=   cjl4 

d 

h2_ 

d 

K 

(1.7)   (2.7) 
-(1.7)  -(2,7) 


(4.61) 


J(2.4)  = 


e' 

J 

a 

=  cia24 

1  -1 
-1     1 

d 

'^22 

d 

H 

(4.62) 


3h 


and   J(2.5)  = 


OAt   3 


By  symmetry  we  have 


=  - [cm22 

At 


eh 


J(3.1)   = 


V^2 
-«1^  «2 


+  cni24 


■hl^  hi 


(4.63) 


=    [   J(1.3)    ]■ 


""l 

^2 

d 

1-  — 1 

\—   — 1 

(4.64) 
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eh 


and 


J(3,2)  = 


m4 


=  [  J(2,3)  ] 


(4.65) 


There  are  two  separate  expressions  for  T(3,3)  given  by 


eh 


J(3.3)  = 


11 
^12 


For  no  switch  on  u^  we  have 


(4.66.1) 


J(3.3)  = 


0  0 
0  0 


(4.66.2) 


For  switch  on  u  we  get  the  result 


J(3,3)  =  cj33 


12 


^11^12 


^11^12   *-ll 


(4.66.3) 


where 


.,,   2  Fmax  At      ,,   , 
cj33=  - sgn(X.   ) 


(4.66.4) 


The  last  two  terms  in  row  3  are  the  submatrices 
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J(3,4)  = 


3' 

J 
a 

0     0 

0     0 

— 

d 

^22 

a 

'•12 

(4.67) 


and 


eh 


J(3,5)  = 


dAt  d 


11 
42 


1 
At 


(5.9) 
(6.9) 


(4.68.1) 


where  the  quantities  (5,9)  and  (.6,9)  for  no  switch  on  u  are  given  by 

(5,9)    =    (2cml3   +cm31)r  +(cnil3  -cm31)r  -  ff^  At   sgn(X      )    (4.68.2) 

1  2    2         11 

and    (6.9)   =    (cml3  -cm31)r^+(2cml3  +cm31)r2-  ^"^^  At  sgn(Xjj)  (4.68.3) 


while  for  switch  on  n  we  get 

(5,9)   =    (2cml3   +  cm31)r  +    (cnil3  -   ecSDr     -  ^"^^  At 


sgn(X.  )[ 


11 


<'   ^12  > 


(4.68.4) 


^^11"  ^12^   2(X^^-  k^^)^ 


and 


(6,9)  =  (cml3  -  cm31)r  +  (2cml3  +  cm31)r  -  ^"'^  At 


sgn(X   )[ 


12 


2    2 
^11"  ^12 


(4.68.5) 


^hr  hl^        2(Xj^-  X^^)^ 
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For  the  fourth  row  of  submatrices  we  have  by  symmetry 


ah 


J(4.1)  = 


a 

d 

d 

_  22_ 

=  [  J(1.4)  ]^  . 


(4.69) 


Sh 


J(4,2)  = 


=  [  J(2.4)  ]   , 


H 

d 

*-22 

(4.70) 


and 


3h 


J(4.3)  = 


=  [  J(3,4)  ]   . 


11 

^12 


^22 


(4.71) 


There  are  two  separate  expressions  for  J(4,4)  given  by 


3h 


J(4.4)  = 


^22 


(4.72.1) 


For  no  switch  on  u  we  have 


J(4,4)  = 


0  0 
0  0 


(4.72.2) 


For  switch  on  a.  we  have  the  result 
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J(4.4)  =  cj44 


hi  ^21^22 

"^21^22   ^21 


(4.72.3) 


where 


...    2  Tmax  At      ,.   . 
cj44=  -  sgnCX-j^) 


(4.72.4) 


The  last  term  in  the  fourth  row  of  submatrices  is 


^h 


J(4,5)  = 


aAt  a  rx 


^22 


1 
At 


(7,9) 
(8,9) 


(4.73.1) 


where  if  no  switch  on  n.  occurs  the  quantities  (7,9)  and  (8,9)  are  given 


by 


(7.9)   =   cm24  9^-  cm24  9^  -  ^°"^^  At    sgnO.^^)  (4.73.2) 


and 


(8,9)   =  -   cm24  9^+  cin24  9^  -  ^'^^'^  At    sgn{\^^)    .  (4.73.3) 


If  a  switch  occurs  on  u.  then  (7,9)  and  (8,9)  are  given  by 
(7.9)  =  cm24  9^-  cm24  9^  -  ^"^^  At  sgn(\^^) 


[. 


*21 


<i'  Ai) 


^hi-  hl^      liX^^-   x^,)" 


(4.74.4) 
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and 


(8,9)    =  -  cm24  9^+  cm24  9^  -  ^"^^  At    sgn(X2^) 


[. 


22 


2  2 

Si"  ^2 


1      . 


<Sr  hi^    2(x^^-  x^^)' 


For  the  last  row  of  snbmatrices  we  have  by  symmetry 


3h 


J(5.1)  = 


=  [  j(i.5)  r  . 


afr: 


dAt 


Bh 


J(5,2)  = 


=  [  J(2.5)  V    . 


dAt 


a^j 


J(5,3)  = 


=  [  J(3.5)  Y    . 


^1] 
^12 


aAt 


Bh 


and 


J(5.4)  = 


=  [  J(4.5)  V 


23 
^22 


aAt 


The  last  term  in  the  fifth  row  of  the  submatrices  is  given  by 

a^j 

J(5.5)   =  I  =  —   [f^  cm22   cml3   +  f^  cm24   cmll 

a^At        At^        ° 


(4.74.5) 


(4.75) 


(4.76) 


(4.77) 


(4.78) 


+  cm31(r^-   r^)^.^^-  \^^)}      . 


(4.79) 
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Assembly  of  element  properties 
to  obtain  system  equations 

To  solve  for  the  unknowns  in  the  whole  solution  region  it  is 

necessary  to  assemble  or  combine  the  element  matrix  equations  to  form 

the  global  equations  governing  the  behavior  over  the  entire  problem 

domain.   The  basis  for  the  assembly  procedure  stems  from  the  fact  that 

at  a  node  where  elements  are  interconnected,  the  values  of  the  unknowns 

are  the  same  for  each  element  sharing  that  node.   The  assembly  is 

performed  by  two  routines.   These  are: 

1.  Node  :  Converts  the  nodal  unknowns  in  the  local  (element) 
numbering  scheme  to  the  nodal  unknowns  in  the  global  (system) 
numbering  scheme  (refer  to  Appendix  II). 

2.  Build  :  Combines  all  the  element  unknowns  to  form  the  system 
unknowns  (refer  to  Appendix  II). 

Application  of  boundary  conditions 
and  solution  of  system  unknowns 

Before  the  system  equations  can  be  solved,  they  must  be  modified 

to  account  for  the  boundary  conditions  of  the  problem,  otherwise  the 

system  matrix  will  be  singular.   The  application  of  the  boundary 

conditions  is  performed  in  routine  Solve  (refer  to  Appendix  II)  where  in 

addition  to  the  boundary  conditions  on  the  states  the  transversal ity 

equation  is  applied  on  the  Lagrangian  multipliers  X.   and  X-  at  both  the 

initial  and  final  time. 
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Application  of  transversalitv  equation 
as  boundary  conditions 

Substituting  equations  (3.67)  and  (3.69)  into  the  transversality 

equation  (3.39)  gives  the  equation 

f(x)  =  Fmax  sgn(X  )X.   +  Tmax  sgn(X.  )X  -1  =  0.       (4.80) 

To  apply  equation  (4.80)  in  the  Hewton-Raphson  iteration  it  should  be  of 
the  form 

^  Ax  =  0  .  (4.81) 

dx 

Differentiating  equation  (4.80)  with  respect  to  X.  gives 

^  =  Fmax  sgn(X  )  .  (4.82) 

Differentiating  equation  (4.80)  with  respect  to  X..  gives 

lf_  =  Tmax  sgn(X.  )  .  (4.83) 

Substituting  equations  (4.82)  and  (4.83)  into  equation  (4.81)  gives 

Fmax   sgn(A,   )A>.     +  Tmax   sgMX   )^k     =   f(x.)  (4.84) 

where    f(x    )    is    equal    to   zero.      The    right    side  vector  elements   f(x.)    at 

t      and    t.   are   modified    in   the   MATRX   subroutine   while    the    left    hand    side 
o  f 

of    equation    (4.84)     is    implemented    in    the    SOLVE    subroutine    (refer 

Appendix   II). 

In  chapter  5  the  results  from  the  continuous  time  and  the  discrete 

time  simulations  are  presented.   The  discrete  case  is  compared  to  the 
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continuous  case.   The  effect  of  varying  the  grid  density,  on  the  final 
time  is  examined.   Recommendations  for  further  study  are  also  presented. 
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CHAPTER  5 

RESULTS  AND  RECOMMENDATIONS 

Introdnction 
In  this  paper  two  methods  of  determining  the  minimum  time  control 
for  the  r-theta  manipulator,  the  continuous  time  method  and  the  discrete 
time  method  were  investigated.  Two  formulations,  the  first  order  and 
the  second  order,  were  used  in  the  continuous  time  method.  In  the 
discrete  time  method  the  second  order  formulation  was  used.  The 
simulations  were  written  in  Fortran  77  and  implemented  on  an  IBM  370 
mainframe  as  well  as  a  HARRIS  H-800  computer.  The  reason  for  using  two 
computer  systems  was  the  availability  of  different  minimization  routines 
on  the  two  systems. 

Continuous  Time  Method 

Four  IMSL  routines  [18]  on  the  IBM  370  were  used  in  the  continuous 
time  simulation.  These  include  DVERK  (a  differential  equation  solver), 
LEQTIF  (a  linear  equations  solver)  and  two  minimization  routines,  ZXJ.JIN 
(a  Quasi-Newton  Method),  and  ZXCGR  (a  conjugate-gradient  method). 

The  continuous  time  problem  required  a  large  number  of  iterations 
in  order  to  get  good  initial  guesses  on  the  unknowns  (initial  values  on 
Xj^,  X,.'  ^3'  ^4  and  final  t  imu  t  ).   A  combination  of  the 

conjugate-gradient  method  and  Newton's  method  was  used.   However,  if  the 


-63- 


initial    guesses    were    not    close    to    the    optimum   values    the    routines 

diverged.      This    difficulty    is    caused   by    the    state    and    multiplier 

equations    being    very    sensitive    to    the    values    of    the    Lagrangian 

multipliers.      If   the   guesses   are    such  that   they  do  not   cause    switches    in 

the    multipliers    then    the    Jacobian  becomes    singular.       It    is    then 

necessary  to   come   up  with  guesses   on  the  multipliers   at    the    initial   time 

that    bring    about    the    correct    number    of    switches    in   order    for    the 

minimization  routine   to  converge. 

The  minimum  time  problem   is    solved   for   three   cases. 

Case   1 

r(t    )    =   1.0    ,  (5.1) 

o 

r(tj)    =   1.0    .  (5.2) 

e(t    )    =  0.0    ,  (5.3) 

o 

and  e(t    )    =  1.5708    .  (5.4) 


Case   2 


Case   3 


r(t    )    =   1.5    ,  (5.5) 

o 


r(t^)    =   1.5    .  (5.6) 


e(t    )    =  0.0    .  (5.7) 

o 


and  e(t^)    =   1.208    .  (5.8) 


r(t    )    =  1.2    .  (5.9) 

o 


r(t^)    =  1.2    .  (5.10) 


e(t    )    =  0.0    ,  (5.11) 

o 
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and  e(t^)  =  1.795  .  (5.12) 


The  velocities  r  and  0  at  both  t   and  t.  are  set  to  zero. 

o      f 

Results  for  the  continuous  time  simulation  for  case  1  are  shown  in 
Figures  5.1  -  5.4. 

Figure  5.1  shows  the  trajectory  of  the  first  joint  variable  0  and 
its  first  and  second  time  derivatives.  These  variables  are  indicated  by 
T,  TD,  and  TDD  respectively,  in  the  figure. 

Figure  5.2  shows  the  trajectory  of  the  second  joint  variable  r  and 
its  first  and  second  time  derivatives.  These  variables  are  indicated  by 
R,  RD,  and  RDD  respectively,  in  the  figure. 

Figure  5.3  illustrates  the  trajectory  of  the  Lagrangian  multiplier 
X^ ,   and  its  first  and  second  time  derivatives.   These  variables  are 

indicated  by  LI,  LDl,  and  LDDl  respectively,  in  the  figure. 

Figure  5.4  illustrates  the  trajectory  of  the  Lagrangian  multiplier 
X,.,   and  its  first  and  second  time  derivatives.   These  variables  are 

indicated  by  L2,  LD2,  and  LDD2  respectively,  in  the  figure. 

All  the  variables  are  seen  to  exhibit  either  even  or  odd  symmetry 
about  t./2.   The  second  derivative  curves  are  not  smooth  at  points 

indicating  the  switchings  of  the  bang  bang  controls. 

Discrete  Time   Method 
Three    routines  were   used    in   the   discrete    time    simulation.      These 
include    LEQTIF    on    the    IBM    370,     and    two    routines    from    Sandia 
Laboratories,      MINA   (a   grid   search  minimization  technique),    and   ODE    (an 
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integration  routine)  which  are  available  on  the  HARRIS.  The  finite 
element  method  also  uses  a  combination  of  techniques.  A  grid  search 
method  is  used  to  give  a  reasonably  good  guess  on  the  final  time  t  . 

This  guess  is  then  used  in  the  finite  element  program  to  start  the 
iterations  on  the  nodal  unknowns.  The  program  then  iterates  until  the 
convergence  criterion  has  been  satisfied.  The  guesses  on  the  multpliers 
have  to  be  of  correct  sign  and  must  constitute  a  symmetrical  path.  The 
guesses  on  the  joint  variables  r  and  9  must  also  constitute  a 
symmetrical  path.  However  the  guesses  on  the  magnitude  of  the 
multipliers  can  be  quite  far  off,  sometimes  over  a  100  percent.  With  a 
reasonable  guess  on  the  time  t  ,  convergence  is  achieved  quite  rapidly. 

Results  for  the  discrete  time  solution  for  the  three  cases  are 
given  in  Table  5.1.  The  number  of  elements  used  for  the  three  cases  is 
twenty  one.  The  effect  of  varying  the  grid  density  in  the  discrete  time 
solution  of  Case  1  are  presented  in  Table  5.2.  Results  for  the  first 
case  are  illustrated  in  Figures  5.5  -  5.8. 

Figure  5.5  shows  the  trajectories  of  9,  and  its  first  and  second 
derivatives.  Figure  5.6  shows  the  trajectories  of  r,  and  its  first  and 
second  derivatives.   Figure  5.7  illustrates  the  trajectories  of  X   ,    and 

its  first  and  second  derivatives.  Figure  5.8  illustrates  the 
trajectories  of  A..,  and  its  first  and  second  derivatives. 

Conclusions  and  Recommendations 
Comparing  the  two  simulations  it  is  seen  that  the  discrete  time 
simulation  agrees  very  closely  with  the  continuous  case,  except  in  the 
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CASE 


1 
2 
3 


TABLE  5.1 

DISCRETE  TIME  SOLUTIONS  FOR  TOE 
MINIMTJJI  FINAL  TILSE 


r(t    ) 
o 

r(t^) 

e(t  ) 

0 

9(t^) 

F 

inal  Tinie(t    ) 

(ft) 

(ft) 

(rad) 

(rad) 

(s) 

1.0 

1.0 

0.0 

1.5708 

1.963 

1.5 

1.5 

0.0 

1.208 

2.499 

1.2 

1.2 

0.0 
TABLE  5. 

,2 

1.795 

2.350 

EFFECT  OF  GRID  DENSITY  ON  FINAL  TIME 
FOR  CASE  1 


SII-niLATION 

continuous  time 
discrete  time 


ITOMBER  OF  ELEMENTS    FINAL  TIME(s) 


21 
42 
84 


1.9644 

1.9629 
1.9640 
1.9643 


second  derivatives  where  the  discrete  time  curves  are  smoother  than  the 
corresponding  continuous  curves.  This  is  due  to  the  linear 
interpolation  function  used  in  the  formulation  of  the  element  equations. 
A  closer  agreement  is  obtained  when  the  grid  density  is  increased  (refer 
Table  5.2).  However  this  also  introduces  corresponding  increases  in 
storage    space   requirements. 

The  finite  element  method  achieved  the  objective  of  applying  a 
discrete  time  method  to  the  solution  of  the  minimum  time  control.  The 
transversal  ity  equation  that  was  enforced  at  both  ends  was  critical  in 
heading  the  iterations  in  the  right  direction.  This  equation  is  not 
only    true    at   the    initial   and  final   time  but   also   at    the    internal   nodes. 
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The  transversality  equation  at  the  internal  nodes  includes  some  velocity 
terms  in  addition  to  the  terms  in  the  equation  at  the  initial  and  final 
time.  An  area  of  further  investigation  could  be  the  application  of  the 
transversality  equation  at  the  internal  nodes.  The  use  of  higher  order 
interpolation  functions  for  the  formulation  of  the  element  equations  can 
also  be  investigated.  Since  a  linear  interpolation  function  was  used  in 
this  paper,  it  does  not  posses  first  derivative  continuity. 

This  thesis  has  investigated  the  application  of  finite  element 
methods  to  the  solution  of  the  minimum  time  problem.  The  deviation  of 
the  discrete  time  solution  from  the  continuous  time  solution  has  been 
investigated  and  found  to  be  reasonable.  Some  recomendat ions  for 
further  investigation  in  the  area  have  been  presented. 
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APPENDIX  I 

DERIVATION  OF  THE  MULTIPLIER  EQUATIONS  FOR 
THE  SECOND  ORDER  FORflULATION 

From  equation  (3.35)  the  performance  index  is  defined  as 


f  "  '2 

J    (x.x.u.X,  t)  =  /   {1+  X, [Fmax  u,  -  mr  +  rare  0 


d     2  * 

+  X-[Tmax  n     -   (mr  0  }  dt  . 

dt 


(a.l) 


The  variation  of  J  with  respect  to  r  and  0  is 

a 


8J  =  dt,+  /   {-  X,m6r  +  mX,e  5r  +  X.,mr2e5e 
a     f   t     1        1        1 
o 


-  X,8  (  ^  (mr^e)  )}  dt  . 

^  dt 


(a. 2) 


Integrating  by  parts  and  rearranging  gives 


5J  =  dt.+  (-  X,m8r  +  X,,  m5r  +  mr2e(8e)X, 
a     f      1       1  1 


2   *    '    2 
-  X  m2r(8r)e  -  X  mr  59  +  X,-mr  69) 


+  /^{5r(-  mX   +  mX  9^+  X  m2r9) 
o 


69(-  ^  (2mr9X  )  -  ^  (X  mr^  ))}  dt  .   (a. 3) 
dt       ^     dt   ^ 
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For  an  extremal  curve 


5J  =  0  . 

a 


(a. 4) 


Setting    the    integrand    in    (a. 3)    equal    to    zero    gives    the    multiplier 
equations 


*  /«  * 


-  mX.+  mX  9  +  X.-m2re  =  0 


(a. 5) 


and 


i_   (2mrex,)    +  t-  (X-mr^)   =  0    . 
dt  ^  dt        ^ 


Therefore,  equation  (a. 4)  reduces  to 


(a. 6) 


5J  =  dt^+  (-X,mSf  +  X,  m6r  +  mr2e(6e)X, 
a    f     1       1  1 


2   *    *   2 
-  X.  m2r(8r)e  -  X.mr  89  +  X.mr  69) 


(a. 7) 
The  variation  of  a  variable  z  at  the  final  time  is  given  by  the  relation 


8x(tj)  =  6x^-  X  dtj 


(a. 8) 


where  5x   is  the  variation  of  the  final  x.  and  x  is  the  slope  of  x  at 
time  t-.   Using  equation  (3.8)  in  (3.7)  gives 


5J  =  dt_+  (X,m  r  -  X.mr  -  mr29  X, 
a     f    1       1  1 


*      '^  * 


2  "         '        2 
+  X  m2r9r  +  X  mr  9  -  X  mr  9) 


-  (-  X  m8r  +  X  m8r  +  mr29X  69 


2  *    *    2 
-  X  m2r96r  -  X.mr  89  +  X  mr  89) 


t=t 


From  equations  (3.21)  and  (3.22) 


-80- 


(a.9) 


Fmax  X     u     =  X^m  r  -  mrG  X^  (a. 10) 

2  " 
and  Tmax  X.   u.   =  X.mr     0  +  X-ia2rf9      .  (a.  11) 


The  initial  and  final  velocities  (f.6)  are  zero  in  the  example  and  the 
initial  and  final  states  are  specified.  Substituting  equations 
(a.lO)and   (a. 11)    into    (a. 9)    together  with  the  boundary  conditions   gives 

1  +  Fmax  X  u     +  Tmax  X  u     =  0      .  (a. 12) 

The  above  equation  is  the  transversality  equation  for  the  second  order 
formulation. 
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i\PPENDIX  II 

FLOW  CHAET  AND   SUBROUTINES  USED 
IN  FINITE  ELEMENT  PROGRATI 


START 

i 

INITIALIZE  VARIABLES 

i 
JIAKE  GUESSES  ON  NODAL  UNKNOV/JIS 

X 
SUBROUTINE  NODE 

i 
START  ITERATIONS 

4 


SUBROUTirffi  SWITCH 

i 
SUBROUTINE  BUILD  < >SUBROUTI^!E  JIATRX 

i 
SUEROUTIIffi  SOLVE 

i 
■NO* CONVERGED   ? 

i 
YES 

I 
STOP 


Figure   A.l      Flow  chart    of 
Finite  Element   Program 
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C      The  MAIN  program  initializez  the  variables,  defines  the  initial 
C     guesses  on  the  unknowns,   calls  the  various  subroutines  and 
C     controls  the  iterations. 

C 

Q  «iji*««***««****«««*«*«**«««*«****«*«******««***4>*4ii|i«****««******««* 

C     Subroutine  NODE  is  used  to  convert  the  local  nodal  unknowns  into 
C     the  global  (  or  system)  unknowns. 
SUBROUTINE  NODE ( NT , NTABLE , MAXELM ) 

INTEGER  NTABLE (MAXELM. 8 ) . NT, MAXELM, NELM, Al , A2 , A3 , A4 , A5 , A6, A7 , A8 
C 

C  NELM  =  element  number 

C  NT  =  Number  of  elements 

C  NTABLE  =  Table  containing  the  global  numbers  for  the  element 

C  unknowns 

C  MAXELM  =  Maximum  number  of  elements 

NELM=0 

DO  10  1=1, NT 

A1=4*(I-1)+1 

A2=Al+4 

A3=4*(I-l)+2 

A4=A3+4 

A5=4*(I-l)+3 


A6=A5+4 

A7=4*(I-l)+4 

A8=A7+4 
C 

NELM=NELM+1 
C 

NTABLE(NELM,1)=A1 

NTABLE(NELM,2)=A2 

NrABLE(NELM.3)=A3 

NTABLE(NELM,4)=A4 

NTABLE(NEU1,5)=A5 

NTABLE(NELM.6)=A6 

NTABLE(NELr.l,7)=A7 

NTABLE(NELM,8)=A8 
C 

10    CONTINUE 
C 

RETURN 
END 

Q  ********«»*•*  *«4i**«**«4i***«*«***i|t**i|i*4i*««*«**«*4c4:*4ii|i*«**4i***«*«««i|i 

C  Subroutine  SWITCH  is  used  to  determine  if  a  switch  of  either 

C  lambdal  or  lambda2  or  both,  has  occured  within  the  element.   The 

C  signs  on  the  lambdas  at  the  tyio   nodes  of  the  element  are  compared. 

C  If  the  signs  are  different,  a  switch  has  occured  within  the 

C  element. 
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Q  ********  «4>*«**««**4*«4i4>***4tifii|i«**««*i|t«««*«*4i*4>***4i******«i»«4i*«***i|i* 

C     Subroutine  IvIATRX  is  used  to  set  up  the  element  matrix  equations 
C     as  well  as  the  element  Jacobian.   The  relations  developed  in 
C     Chapter  4  are  used  in  this  routine. 

C     Subroutine  BUILD  is  used  to  assemble  the  element  Jacobian  into  the 
C     global  Jacobian. 

SUBROUTINE  BUILD (NT. NTABLE, ITABLE, JAC, RSV. SGNLl . SGNL2 ,TS1 . TS2 . GJ , M 
•AXRG,  RSVG,  MAXELM,  JIAT.  R,  THETA,  LBl .  LB2 ,  flAXN,  DT) 

Q  *********  *«****«****«****4i*******««****i|i«****«*«««««*««**«««**4c**« 

REAL*  8  J AC ( 9 . 9 ) . RS V ( 9 ) , SGNLl , SGNL2 . TSl ( MAXELM ) , TS2 ( MAXELM ) 

RE AL*  8  GJ ( MAXRG , MAXRG ) , RS VG ( MAXRG ) , MAT ( 9 , 9 ) 

REAL*8  R(MAXN) ,TnETA(MAXN) , LBl (MAXN) , LB2 (MAXN) . DT 

INTEGER  NT, NTABLE (MAXELM. 8 ) . ITABLE ( 8 ) , II , 12 
C     GJ  is  the  global  jacobian. 
C     RSVG  is  the  global  right  side  vector 
C     NT  is  the  number  of  elements 
C 

C     ARRAYS  ARE  INITIALIZED 
C 

DO  70  I=1.4*NT+5 
DO  80  J=1.4*NT+5 
GJ(I,J)=0.0 
80      CONTINUE 
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RSVG(I)=0.0 
70    CONTINUE 
C 

C     THE  GLOBAL  JACOBIAN  AND  GLOBAL  RIGHT  SIDE  VECTOR  ARE  ASSEJffiLED 
C 

DO  10  11=1, NT 
DO  20  J=l,8 

ITABLE ( J ) =NTABLE ( II , J ) 
20      CONTINDE 
C 

C       THE  ELEJBENT  MATRIX, JACOBIAN  AND  RIGHT  SIDE  VECTOR  ARE  OBTAINED 
C 

CALL  MATRX (R, MAXN, THETA, LBl , LB2 , DT, NT, SGNLl , SGNL2 , TSl , TS2 ,  MAXELM. M 
•AT,RSV,JAC,I1) 

C     Tlie  element  jacobians  and  the  element  right  side  vectors  are 
C      assembled  into  the  global  Jacobian  and  the  global  right  side 
C      vector  here. 

DO  30  12=1,8 
DO  40  J=l,8 

GJ ( ITABLE ( 12 ) , ITABLE ( J ) ) =JAC ( 12 . J ) +GJ ( ITABLE ( 12 ) , ITABLE (J ) ) 
40      CONTI^^JE 

GJ ( ITABLE ( 12 ) , 4*NT+5 ) =JAC ( 12 , 9 ) +GJ ( ITABLE ( 12 ) , 4*NT+5 ) 
RSVG ( ITABLE ( 12 ) ) =RSV( 12 ) +RS VG ( ITABLE ( 12 ) ) 
30    CONTINUE 
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DO  95  J=l,8 

GJ (4*NT+5 . ITAELE(J) )=JAC (9, J)+GJ (4*NT+5 , ITABLE( J) ) 
95    CONTINDE 

GJ (4*NT+5 . 4*NT+5 ) =JAC (9,9) +GJ (4*NT+5 , 4*NT+5 ) 

RSVG(4«NT+5)=RSV(9)+RSVG(4*NT+5) 
C 
10    CONTINUE 

RETURN 
END 

C     Subroutine  SOLVE  is  used  to  apply  the  boundary  conditions  and  the 
C     transversality  equation  on  the  global  jacobian.   The  system  of 
C     equations  are  solved  by  the  IMSL  routine  LEQTIF  (gaussian 
C     elimination  technique)  and  the  guesses  are  updated.   The 
C     convergence  criteria  is  also  computed  here. 

SUBROUTINE  SOLV(GJ , RSVG, MAXRG. NT. MRK, YfK,  R, THETA, LBl , LB2 , IIAXN.  CONV, 
•DT ,  NTABLE ,  MAXELM ,  ITAELE ,  K ) 

Q  4ii|i«*««««***********«*****«*««*l»*«*******«*«*«***«****««****«*««*** 

REAL*8  GJ  (IIAXRG, MAXRG) ,  RSVG(llAXRG) ,  W(MRE) ,  PI,  CONVI^,  CONVD 
REAL*8  R(MXN) , THETA ( JIAXN ),  LBl  (MAXN)  ,LB2  (MAX^n  ,DT,  CONV,  A,  B,  C 
INTEGER  Ml ,  N2 ,  lA,  IDGT,  lER,  NTABLE  (MXELM,  8 ) ,  ITABLE  { 8 ) ,  K 

C     GJ  is  the  global  Jacobian 

C     RSVG  is  the  global  right  side  vector 

C     NT  is  the  number  of  elements. 

C     R  is  the  Joint  2  variable 
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C     Theta  is  the  joint  1  variable 

C     LBl  is  lambdal 

C     LB2  is  lambda2 

C     DT  is  the  length  of  element 

C     CONV  is  the  convergence  criteria 

C 

CONVN=  0.0 

CONVD=  0.0 

corw=  0.0 
c 

C     BOUI'TOARY  CONDITIONS  ARE  SPECIFIED 
C 

DO  60  1=1,3 

DO  60  J=l,4*NT+5 
GJ(I,J)=  0.0 
GJ(4*NT+I,J)=  0.0 
60      CONTINDE 
DO  65  1=1,2 
RSVG(I)=  0.0 
RSVG(4*NT+I)=  0.0 
65    CONTINUE 
C 

DO  68  1=1,2 
GJ(I,I)=  1.0 
GJ(4*OT+I,4*NT+I)=  1.0 
68    CONTINUE 
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c 

C  TRANSVERSALITY  EQUATION  APPLIED  AT  THE  INITIAL  AND  FINAL  TIME 

C  known  data 

C  Fmax  =  1 

C  Tmax  =  1 

C  m  =  1 

C     at  t  =  t   sgn(LBl  )  =  1 
o 

C  sgn(LB2)  =  -1 

C     at  t  =  t   sgn(LBl  )  =  1 

C  sgn(LB2  )  =  1 

Substituting  the  above  values  into  equation  (4.83)  gives 
C 

GJ(3,3)=  1.0 
GJ(3,4)=  -1.0 
GJ(4*NT+3,4*NT+3)=  1.0 
GJ(4*NT+3,4*NT+4)=  1.0 
C 

C     GLOBAL  JACOBIAfJ  IS  SOLVED 
C 

Ml=l 

N2=4*NT+5 

IA=MAXRG 

IDGT=0 

CALL  LEQTIF  ( GJ ,  Ml ,  N2 , 1 A ,  RS  VG .  IDGT ,  ^TK .  lER ) 
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C     UPDATE  NODAL  VARIABLES 
C 

DO  20  13=1, NT+1 

R(I3)=  R(I3)-RSVG(4*(I3-1)+1) 

THETA(I3)=  THETA(I3)-RSVG(4*(I3-l)+2) 

LB1(I3)=  LBl(I3)-RSVG(4*(I3-l)+3) 

LB2(I3)=  LB2(I3)-RSVG(4*(I3-l)+4) 
20  CONTirJDE 

DT=DT-RS VG ( 4  *NT+5 ) 
C 
C  CONVERGENCE  CRITERION  IS  COflPDTED.        Refer  equatioii(4.53) 


C 


C 


c 


c 


DO  30  1=1, NT+1 

CONVN=  C0rfVN+(RSVG(4*(I-l)+l)**2) 
CONVD=  C0NVD+(R(I)**2) 

CONVN=  C0NVN+(RSVG(4*(I-l)+2)**2) 

CONVD=  C0NVD+(THETA(I)«*2) 

COm'N=  CONVN+(RSVG(4»(I-l)+3)**2) 
CONVD=  C0NVD+(LB1(I)*»2) 

CONVN=  C0NVN+(RSVG(4*(I-l)+4)**2) 
CONVD=  C0NVD+(LB2(I)**2) 
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30    CONTINUE 
C 

CONVN=  CONVN+(RSVG(4*NT+5)**2) 

CONVD=  C0NVD+(DT**2) 
C 

CONV=  CONVN/CONVD 

CONV=  DSQRT(CONV) 

RETURN 
END 
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ABSTRACT 

This  paper  investigates  the  minimum  time  control  of  a  two  degree 
of  freedom  manipulator  subject  to  control  magnitude  constraints.  Two 
methods  of  solution,  a  continuous  time  method  and  a  discrete  time  method 
are  applied,  and  their  relative  merits  are  examined. 

The  mathematical  model  for  the  r-9  manipulator,  a  two  degree  of 
freedom  manipulator  operating  in  the  horizontal  z-y  plane  is  developed. 

The  necessary  conditions  for  the  minimum  time  control  of  the 
manipulator  are  presented.  Two  formulations,  the  first  and  the  second 
order  formulations,  are  used. 

The  finite  element  method  is  developed  for  the  discrete  time 
simulation  of  the  time  optimal  control  of  the  manipulator.  A 
combination  of  a  grid  search  technique  and  Mewton-Raphson  iteration  on 
the  finite  element  equations  is  used  to  obtain  the  minimum  time  with  the 
state  and  control  histories  .  The  discrete  time  solution  is  compared  to 
the  continuous  time  solution.  The  results  of  the  computer  simulations 
are  presented,  as  well  as  recommendations  for  further  study. 


